clc;clear;clearvars;
% 随机生成5个数据
num_initial = 10;
num_vari = 30;
% 搜索区间
upper_bound = 5.12;
lower_bound = -5.12;
iter = 20000;
w = 1;
% 领域大小
k = 2;
% 随机生成5个数据,并获得其评估值
sample_x = lhsdesign(num_initial, num_vari).*(upper_bound - lower_bound) + lower_bound.*ones(num_initial, num_vari);
sample_y = Rastrigin(sample_x);
Fmin = zeros(iter, 1);
aver_Fmin = zeros(iter, 1);

for n = 1 : 100
    k1 = 1;
    % 初始化一些参数
    pbestx = sample_x;
    pbesty = sample_y;
    % 当前位置信息presentx
    presentx = lhsdesign(num_initial, num_vari).*(upper_bound - lower_bound) + lower_bound.*ones(num_initial, num_vari);
    vx = sample_x;
    fprintf("n: %.4f\n", n);
    lbest = zeros(num_initial, num_vari);
    for i = 1 : iter
        index = 1;
        r = rand(num_initial, num_vari);
        % pso更新下一步的位置,这里可以设置一下超过搜索范围的就设置为边界
        vx = w.*vx + 2 * r .* (pbestx - presentx) + 2 * r .* (lbest - presentx);
        vx(vx > upper_bound) = upper_bound;
        vx(vx < lower_bound) = lower_bound;
        presentx = presentx + vx;
        presentx(presentx > upper_bound) = upper_bound;
        presentx(presentx < lower_bound) = lower_bound;
        presenty = Rastrigin(presentx);
        % 更新每个单独个体最佳位置
        pbestx(presenty < pbesty, :) = presentx(presenty < pbesty, :);
        pbesty(presenty < pbesty, :) = presenty(presenty < pbesty, :);
        % 更新每个个体的lbest
        n1 = ceil(k / 2);
        n2 = floor(k / 2);
        % 处理开头部分
        for i1 = 1 : n1
            [~, ind] = min(pbesty(1 : (1 + k), 1));
            lbest(index, :) = pbestx(ind, :);
            index = index + 1;
        end
        % 处理中间部分
        for i2 = 1 : (num_initial - k)
            [~, ind] = min(pbesty(i2 : (i2 + k), 1));
            lbest(index, :) = pbestx(i2 + ind - 1, :);
            index = index + 1;
        end
        % 处理结尾部分
        for i3 = 1 : n2
            [~, ind] = min(pbesty((num_initial - k) : num_initial, 1));
            lbest(index, :) = pbestx(num_initial - k + ind - 1, :);
            index = index + 1;
        end
        [fmin, ~] = min(pbesty);
        % fprintf("iter %d fmin: %.4f\n", i, fmin);
        Fmin(k1, 1) = fmin;
        k1 = k1 +1;
    end
    aver_Fmin = aver_Fmin + Fmin;
end
aver_Fmin = aver_Fmin ./ 100;
% disp(pbestx(gbest, :));
plot(aver_Fmin);